from Methods.block import *
import numpy as np

# 输入8个点，输出3个点
def point_8_to_3(args):
    a = args[7]
    b = args[1]
    c = [(a[0] + b[0]) / 2, (a[1] + b[1]) / 2, (a[2] + b[2]) / 2]
    args = [a, c, b]

    return args


# 输入起点终点宽高，输出8个点
def dian2(p1, p2, width, height):  # 第二种即两个中心点z不一样
    u = 0
    p3 = np.zeros(3)
    p4 = np.zeros(3)
    p5 = np.zeros(3)
    p6 = np.zeros(3)
    p_1 = np.zeros(3)
    p_2 = np.zeros(3)
    p_3 = np.zeros(3)
    p_4 = np.zeros(3)
    p_5 = np.zeros(3)
    p_6 = np.zeros(3)
    p_7 = np.zeros(3)
    p_8 = np.zeros(3)
    if p1[1] - p2[1] == 0:
        u = 1
    if u == 0:
        k = -(p1[0] - p2[0]) / (p1[1] - p2[1])
        x = width / (2 * (1 + k ** 2) ** 0.5)
        p3[0] = p1[0] + x
        p3[1] = p1[1] + k * x
        p3[2] = p1[2]
        p4[0] = p1[0] - x
        p4[1] = p1[1] - k * x
        p4[2] = p1[2]
        p5[0] = p2[0] + x
        p5[1] = p2[1] + k * x
        p5[2] = p2[2]
        p6[0] = p2[0] - x
        p6[1] = p2[1] - k * x
        p6[2] = p2[2]
    else:
        p3[0] = p1[0]
        p3[1] = p1[1] + width / 2
        p3[2] = p1[2]
        p4[0] = p1[0]
        p4[1] = p1[1] - width / 2
        p4[2] = p1[2]
        p5[0] = p2[0]
        p5[1] = p2[1] + width / 2
        p5[2] = p2[2]
        p6[0] = p2[0]
        p6[1] = p2[1] - width / 2
        p6[2] = p2[2]
    a = (p2[1] - p1[1]) * (p3[2] - p1[2]) - (p3[1] - p1[1]) * (p2[2] - p1[2])
    b = (p2[2] - p1[2]) * (p3[0] - p1[0]) - (p3[2] - p1[2]) * (p2[0] - p1[0])
    c = (p2[0] - p1[0]) * (p3[1] - p1[1]) - (p3[0] - p1[0]) * (p2[1] - p1[1])
    t = height / (2 * (a ** 2 + b ** 2 + c ** 2) ** 0.5)
    p_3[0] = p3[0] + a * t
    p_3[1] = p3[1] + b * t
    p_3[2] = p3[2] + c * t
    p_2[0] = p3[0] - a * t
    p_2[1] = p3[1] - b * t
    p_2[2] = p3[2] - c * t
    p_4[0] = p4[0] + a * t
    p_4[1] = p4[1] + b * t
    p_4[2] = p4[2] + c * t
    p_1[0] = p4[0] - a * t
    p_1[1] = p4[1] - b * t
    p_1[2] = p4[2] - c * t
    p_7[0] = p5[0] + a * t
    p_7[1] = p5[1] + b * t
    p_7[2] = p5[2] + c * t
    p_6[0] = p5[0] - a * t
    p_6[1] = p5[1] - b * t
    p_6[2] = p5[2] - c * t
    p_8[0] = p6[0] + a * t
    p_8[1] = p6[1] + b * t
    p_8[2] = p6[2] + c * t
    p_5[0] = p6[0] - a * t
    p_5[1] = p6[1] - b * t
    p_5[2] = p6[2] - c * t
    list = [p_3, p_7, p_6, p_2, p_4, p_8, p_5, p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    # print(new)
    return new


# 输入管道2个点，面的4个点，return碰撞信息；
def pengzhuang(mian, xian):
    n = mian[0]
    m = mian[1]
    o = mian[2]
    q1 = xian[0]
    q2 = xian[1]
    p = ((n[1] - m[1]) * (o[2] - m[2]) - (o[1] - m[1]) * (n[2] - m[2]),
         (n[2] - m[2]) * (o[0] - m[0]) - (o[2] - m[2]) * (n[0] - m[0]),
         (n[0] - m[0]) * (o[1] - m[1]) - (o[0] - m[0]) * (n[1] - m[1]))
    P = (p[0] ** 2 + p[1] ** 2 + p[2] ** 2) ** 0.5
    d1 = (p[0] * (q1[0] - m[0]) + p[1] * (q1[1] - m[1]) + p[2] * (q1[2] - m[2]))
    d2 = (p[0] * (q2[0] - m[0]) + p[1] * (q2[1] - m[1]) + p[2] * (q2[2] - m[2]))
    if d1 == d2:
        return None
    else:
        u = d1 / (d1 - d2)
        t = [q1[0] - u * (q1[0] - q2[0]), q1[1] - u * (q1[1] - q2[1]), q1[2] - u * (q1[2] - q2[2])]
        j = -1
        h = 0
        while j < 2:
            j = j + 1
            if t[j] > max(m[j], n[j], o[j]) or t[j] < min(m[j], n[j], o[j]):
                h = 1
            elif t[j] > max(q1[j], q2[j]) or t[j] < min(q1[j], q2[j]):
                h = 1
        if h == 0:
            return t
        else:
            return None


#输入管道信息，输出四条与流向平行的棱
def pipe_to_pipes(pipe):
    import numpy as np
    a = dian2(pipe[0],pipe[1],pipe[2],pipe[3])
    a1 = [a[0],a[1]]
    a2 = [a[3],a[2]]
    a3 = [a[4], a[5]]
    a4 = [a[7], a[6]]
    a5 = [pipe[0],pipe[1]]
    a6 = [a1,a2,a3,a4,a5]
    # print(a6)
    return a6



# 输入管道信息与障碍物信息，输出管道与障碍物的交点
def intersection_to_point(a, b):
    a1 = pipe_to_pipes(a)
    b1 = dian2(b[0], b[1], b[2], b[3])
    b2 = point_8_to_3(b1)
    b3 = point_3_to_surface_6(b2)  # 六个面的顺序为前后上下左右
    e = []
    point1 = []

    for v in range(len(b3)):
        for w in range(len(a1)):
            pipe = a1[w]
            c = b3[v]
            d = pengzhuang(c, pipe)
            # print(d)
            e.append(d)
    if abs(a[1][0] - a[0][0]) > abs(a[1][1] - a[0][1]) and abs(a[1][0] - a[0][0]) > abs(a[1][2] - a[0][2]):
        if a[1][0] - a[0][0] > 0:#左面
            point1 = e[20:25]
        else:#右面
            point1 = e[25:30]
    elif abs(a[1][1] - a[0][1]) > abs(a[1][0] - a[0][0]) and abs(a[1][1] - a[0][1]) > abs(a[1][2] - a[0][2]):
        if a[1][1] - a[0][1] > 0:#后面
            point1 = e[5:10]
        else:#前面
            point1 = e[0:5]
    elif abs(a[1][2] - a[0][2]) > abs(a[1][0] - a[0][0]) and abs(a[1][2] - a[0][2]) > abs(a[1][1] - a[0][1]):
        if a[1][2] - a[0][2] > 0:#下面
            point1 = e[15:20]
        else:#上面
            point1 = e[10:15]
    point = list(filter(None, point1))
    if len(point) == 0:
        if abs(a[1][0] - a[0][0]) > abs(a[1][1] - a[0][1]) and abs(a[1][0] - a[0][0]) > abs(a[1][2] - a[0][2]):
            point2 = e[10:20]
            point = list(filter(None, point2))
        elif abs(a[1][1] - a[0][1]) > abs(a[1][0] - a[0][0]) and abs(a[1][1] - a[0][1]) > abs(a[1][2] - a[0][2]):
            point2 = e[10:20]
            point = list(filter(None, point2))
        elif abs(a[1][2] - a[0][2]) > abs(a[1][0] - a[0][0]) and abs(a[1][2] - a[0][2]) > abs(a[1][1] - a[0][1]):
            point2 = e[20:30]
            point = list(filter(None, point2))
    # print(point)
    return point

# 输入管道信息与障碍物信息，输出管道与障碍物面的点
def intersection_to_mian(a, b):
    a1 = pipe_to_pipes(a)
    b1 = dian2(b[0], b[1], b[2], b[3])
    b2 = point_8_to_3(b1)
    b3 = point_3_to_surface_6(b2)  # 六个面的顺序为前后上下左右
    e = []
    point = []
    point1 = []
    mian = []
    mian1 = []
    for v in range(len(b3)):
        for w in range(len(a1)):
            pipe = a1[w]
            c = b3[v]
            d = pengzhuang(c, pipe)
            e.append(d)
    mian1 = intersection_to_point(a,b)
    for i in range(6):
        point = e[5*i:5*i+5]
        point1 = list(filter(None,point))
        if point1 == mian1:
            mian = b3[i]
    # print(mian)
    return mian


